#include "canbus/message.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @class CanMsg21
 * @brief Concrete can message that stores vehicle control information
 */
class CanMsg21: public CanMsg {
public:
  CanMsg21():
  CanMsg(),
  gantry_speed(0),
  gantry_angle(0)
  {
    id_ = 21;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  CanMsg21(uint8_t* value): 
  CanMsg(value), 
  gantry_speed(0),
  gantry_angle(0)
  {
    id_ = 21;
    for(int i=0;i<8;i++) {
      *(data_+i) = 0;
    }
  }

  virtual ~CanMsg21() = default;

  void UpUpdateData() {}

  template <typename T>
  void DownUpdate(const T& input_data) {

    gantry_speed = mathClamp<double>(input_data.gantry_speed, -kVehicleMaxSpeed, kVehicleMaxSpeed)
     * speed_unit + speed_offset;
    gantry_angle = mathClamp<double>(input_data.gantry_angle, -kVehicleMaxAngle, kVehicleMaxAngle) 
     * speed_unit + angle_offset;


    *data_ = getTwin(gantry_speed, 1);
    *(data_+1) = getTwin(gantry_speed, 0);
    *(data_+2) = getTwin(gantry_angle, 1);
    *(data_+3) = getTwin(gantry_angle, 0);
    *(data_+4) = input_data.gantry_mode;
  }

  static const int32_t ID;
  static const std::string NAME;

  double gantry_speed;
  double gantry_angle;

  const int speed_unit = 10;
  const int speed_offset = 1250;
  const int angle_unit = 100;
  const int angle_offset = 10000;

  friend class CanbusHandler;
};

// const int32_t CanMsg21::ID = 21;
// const std::string CanMsg21::NAME = "control_vehicle";

}